
#include "stm32f10x.h"
#include <stdlib.h>
#include "MyProject.h"
#include "adc.h"

/************************************************




=================================================


************************************************/
/******************************************************************************/
#define LED_blink    GPIOC->ODR^=(1<<13)
/******************************************************************************/
float target;//目标值，现在给的是q轴电压
unsigned int T=50;//周期ms
float M=0.2;//调制比0~1
DQCurrent_s current_cal;

unsigned int ShanQu=0;
float ratchet_center_angle=0.0;
float ratchet_center_dead_angle=1.0/180*_2PI;//棘轮中点的切换的重叠角度，滞环比较
float ratchet_boundary_angle=15.0/180*_2PI;;
float ratchet_step_angle=10.0/180*_2PI;//棘轮中点的间隔角度

float force=0;
float force_MAX=1;
u8 flag=0;

/******************************************************************************/
void commander_run(void);
/******************************************************************************/
void MOTOR_PWM_OFF(void)
{
	TIM_SetCompare1(TIM2,0*PWM_Period);
	TIM_SetCompare2(TIM2,0*PWM_Period);
	TIM_SetCompare3(TIM2,0*PWM_Period);
}
	
void GPIO_Config(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC|RCC_APB2Periph_AFIO, ENABLE);//使能GPIOA,GPIOB,GPIOC,AFIO;
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;         //PC13是LED
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;   //推挽输出	
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;  //速度
	GPIO_Init(GPIOC, &GPIO_InitStructure);             //对选中管脚初始化
	GPIO_SetBits(GPIOC,GPIO_Pin_13);                   //上电点亮LED
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;          //PB9是电机使能
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	GPIO_SetBits(GPIOA,GPIO_Pin_6);                  //低电平解除,Motor_init()中使能
}

float T0,T1,T2;
float Ta,Tb,Tc;



void setPhaseVoltage_my(float Uq, float Ud, float angle_el)
{
	float Uout;
	uint32_t sector;
	
	
	if(Ud) // only if Ud and Uq set 
	{// _sqrt is an approx of sqrt (3-4% error)
		Uout = _sqrt(Ud*Ud + Uq*Uq) / voltage_power_supply;
		// angle normalisation in between 0 and 2pi
		// only necessary if using _sin and _cos - approximation functions
		angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
	}
	///////////////////////////////////////////////////////，Ud=0，控制Uq
	else
	{// only Uq available - no need for atan2 and sqrt
		Uout = Uq / voltage_power_supply;
		// angle normalisation in between 0 and 2pi
		// only necessary if using _sin and _cos - approximation functions
		angle_el = _normalizeAngle(angle_el + _PI_2);//只要求产生垂直于当前角度的目标方向
	}
	///////////////////////////////////////////////////////
	if(Uout> 0.2)Uout= 0.2;
	if(Uout<-0.2)Uout=-0.2;
	
	sector = (angle_el / _PI_3) + 1;
	T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
	T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout;
	T0 = 1 - T1 - T2;
	
	// calculate the duty cycles(times)
	switch(sector)
	{
		case 1:
			Ta = T1 + T2 + T0/2;
			Tb = T2 + T0/2;
			Tc = T0/2;
			break;
		case 2:
			Ta = T1 +  T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T0/2;
			break;
		case 3:
			Ta = T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T2 + T0/2;
			break;
		case 4:
			Ta = T0/2;
			Tb = T1+ T0/2;
			Tc = T1 + T2 + T0/2;
			break;
		case 5:
			Ta = T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T2 + T0/2;
			break;
		case 6:
			Ta = T1 + T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T0/2;
			break;
		default:  // possible error state
			Ta = 0;
			Tb = 0;
			Tc = 0;
	}
	
	TIM_SetCompare1(TIM2,Ta*PWM_Period);
	TIM_SetCompare2(TIM2,Tb*PWM_Period);
	TIM_SetCompare3(TIM2,Tc*PWM_Period);
}
//PID参数：
float pid_current_P_my=0.05;
float pid_current_I_my=0.05;
float integral_current_prev_my=0;
float proportional,integral,out;

float pid_current_P_my_d=0.1;
float pid_current_I_my_d=0.01;
float integral_current_prev_my_d=0;
float proportional_d,integral_d,out_d;

float PID_current_my(float error){
	
	proportional = pid_current_P_my * error;
	integral = integral_current_prev_my + error;
	
	if(integral*pid_current_I_my>1) integral=1/pid_current_I_my;
	if(integral*pid_current_I_my<-1) integral=-1/pid_current_I_my;
	integral_current_prev_my=integral;
	
	out=proportional+pid_current_I_my*integral;
	return  out;

}
float PID_current_my_d(float error){
	
	proportional_d = pid_current_P_my_d * error;
	integral_d = integral_current_prev_my_d + error;
	
	if(integral_d*pid_current_I_my_d>1) integral_d=1/pid_current_I_my_d;
	if(integral_d*pid_current_I_my_d<-1) integral_d=-1/pid_current_I_my_d;
	integral_current_prev_my_d=integral_d;
	
	out_d=proportional_d+pid_current_I_my_d*integral_d;
	return  out_d;

}
void current_QD_SET(float IQ,float ID){
	
	electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
		current_ADC.b=-((float)Get_Adc_Average(ADC_Channel_3,1)*(3.3/4096)-1.65)*0.4;//1÷0.05÷50=0.4
		current_ADC.a=0-current_ADC.b+((float)Get_Adc_Average(ADC_Channel_4,1)*(3.3/4096)-1.65)*0.4;	
			
		current_cal = getFOCCurrents(_normalizeAngle(electrical_angle));
		// filter values
//		current.q = LPFoperator(&LPF_current_q,current.q);
//		current.d = LPFoperator(&LPF_current_d,current.d);
		// calculate the phase voltages
		
		voltage.d = PID_current_my_d( ID-current_cal.d);//在这里设置d轴电流
		voltage.q = PID_current_my(IQ - current_cal.q); //在这里设置q轴电流
		
		setPhaseVoltage(voltage.q, voltage.d , electrical_angle);
		//电流闭环	
}

	float Angle_now=0;
int main(void)
{
	unsigned int count_i=0;
	//float Ta,Tb,Tc;
	float Angle_last=0;

	unsigned short int adcx1,adcx2;
	float Ua,Ub;	
	float dir=0;
	float angle_target=0;
	float angle_nor=0;

	GPIO_Config();//初始化PC13是LED、PC13是LE
	uart_init(115200);//串口初始化，串口1初始化，PA9和PA10
	
	I2C_Init_();               //AS5600，PB6和PB7
	printf("AS5600\r\n");

	TIM2_PWM_Init();//PWM高级定时器初始化，PA0\PA1\PA2

	delay_ms(100);
	
	MagneticSensor_Init();     //AS5600 or TLE5012B，磁编码器初始化
	
	Adc_Init();		  		//ADC初始化
	
	voltage_power_supply=10;   //V，需要根据这个值给定Uq,Ud?
	voltage_limit=4;           //V，最大值需小于12/1.732=6.9,即电压的有效值极值
	velocity_limit=10;         //rad/s angleOpenloop() and PID_angle() use it
	//校准电机需要的Uq
	voltage_sensor_align=1;    //V     alignSensor() and driverAlign() use it，大功率电机0.5-1，小功率电机2-3，
	torque_controller=Type_voltage;  //当前只有电压模式，Type_voltage=0
	controller=Type_angle;  //Type_angle; 先用角度控制使的角度启动初始化
	target=0;
	
	pole_pairs=11;
	Motor_init();//打开PB9使能，voltage_sensor_align还是3
	Motor_initFOC();//电机启动初始化
	PID_init();//PID参数初始化
  //printf("Motor ready.\r\n");
	MOTOR_PWM_OFF();//使用关闭PWM来停转电机，可以省电0.1W
	shaft_velocity_sp = 0;
	angle_target = shaftAngle();// shaft angle;
	
	TIM3_1ms_Init();           //interrupt per 1ms,
	
	systick_CountMode();   //不能再调用delay_us()和delay_ms()函数
	ratchet_center_angle=shaftAngle();
	while(1)
	{
		count_i++;
		Angle_now=getAngle();

		if(time1_cntr>=1000)  //1000ms，在TIM3_IRQHandler中++,200*1ms=200ms
		{
			time1_cntr=0;
			
		}
		if(time2_cntr>=100)//在TIM3_IRQHandler中++,
		{
			LED_blink;
			time2_cntr=0;
		}
		
		//shaft_velocity = shaftVelocity();
		// read dq currents
		shaft_angle = shaftAngle();// shaft angle	
		electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
		//棘轮旋钮：给不同位置施加不同的电流（转矩）
		//棘轮模式，中点无输出0，边界给力矩1,2，超过了阈值切换中点3,4
		if (shaft_angle >= ratchet_center_angle - ratchet_center_dead_angle && 
        shaft_angle <= ratchet_center_angle + ratchet_center_dead_angle) 
		{
			setPhaseVoltage(0,0, electrical_angle);
			flag=0;
		}
		else if (shaft_angle <= ratchet_center_angle-ratchet_boundary_angle) 
		{
			ratchet_center_angle=shaft_angle-ratchet_step_angle;
		}
		else if (shaft_angle >= ratchet_center_angle+ratchet_boundary_angle) 
		{
				ratchet_center_angle=shaft_angle+ratchet_step_angle;
		}
		else
		{
			if (shaft_angle <= ratchet_center_angle)
			{
				force=(shaft_angle-ratchet_center_angle+ratchet_center_dead_angle)*force_MAX/ratchet_boundary_angle*4;
				force=-force;
				if(force>force_MAX)force=force_MAX;
					setPhaseVoltage(force,0, electrical_angle);//为正的力
				flag=1;
			}
			if (shaft_angle >= ratchet_center_angle)
			{
				force=(shaft_angle-ratchet_center_angle-ratchet_center_dead_angle)*force_MAX/ratchet_boundary_angle*4;
				force=-force;
				if(force<-force_MAX)force=-force_MAX;
				setPhaseVoltage(force,0, electrical_angle);//为负的力
				flag=2;
			}
		}
		//current_QD_SET(0.2,0);
	}
}
/******************************************************************************/
void commander_run(void)
{
	if((USART_RX_STA&0x8000)!=0)
	{
		switch(USART_RX_BUF[0])
		{
			case '1':
				printf("Hello World!\r\n");
				M=M-0.01;
				break;
			case '2':   //T6.28
				target=atof((const char *)(USART_RX_BUF+1));
				printf("RX=%.4f\r\n", target);
				M=M+0.01;
				break;
			case '3':   //D
				printf("OK!\r\n");
				T=T-1;
				break;
			case '4':   //E
				printf("OK!\r\n");
				T=T+1;
				break;
		}
		if(M>0.8)M=0.8;
		if(M<0.0)M=0.0;
		if(T>1000)T=2000;
		if(T<5)T=10;
		USART_RX_STA=0;
	}
}
/******************************************************************************/



